#include <string.h>
#include <iostream>
#include <sys/socket.h>
#include <cstring>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include "hikvision_ir_camera.hpp"

HikvisionIRCamera::HikvisionIRCamera()
{
    init_camera_handle_t(&camera_handle);
    init_raw_data_t(&raw_data);
}

HikvisionIRCamera::~HikvisionIRCamera()
{
    release_memery(&camera_handle, &raw_data);
}

void HikvisionIRCamera::init_camera_handle_t(camera_handle_t* camera_handle)
{
    camera_handle->lUserID = -1;
    camera_handle->lRealPlayHandle = -1;
    camera_handle->running_ = false;
    camera_handle->is_connected = false;
    camera_handle->loginInfo = {0};
    camera_handle->deviceInfo = {0};

    std::memset(&camera_handle->loginInfo, 0, sizeof(camera_handle->loginInfo));
    std::memset(&camera_handle->deviceInfo, 0, sizeof(camera_handle->deviceInfo));

    // 填充 loginInfo 和 deviceInfo 的字段
    std::strncpy(camera_handle->loginInfo.sDeviceAddress, "192.168.1.64", sizeof(camera_handle->loginInfo.sDeviceAddress) - 1);
    camera_handle->loginInfo.wPort = 8000;
    std::strncpy(camera_handle->loginInfo.sUserName, "admin", sizeof(camera_handle->loginInfo.sUserName) - 1);
    std::strncpy(camera_handle->loginInfo.sPassword, "JIQIREN1234", sizeof(camera_handle->loginInfo.sPassword) - 1);
}

void HikvisionIRCamera::init_raw_data_t(raw_data_handle_t* raw_data)
{
    raw_data->frame_data_start_index = 0;
    raw_data->frame_data_end_index = 0;
    raw_data->frame_buffer = (uint8_t *)calloc(BUFFER_SIZE, sizeof(uint8_t));
    raw_data->buffer_size = BUFFER_SIZE;
    raw_data->is_buffer_empty = true;
    raw_data->max_frame_queue_size = MAX_FRAME_QUEUE_SIZE;
    raw_data->running_ = false;
    raw_data->grey_8bit_image = (uint8_t *)calloc(CAMERA_WIDTH * CAMERA_HEIGHT, sizeof(uint8_t));
    raw_data->grey_16bit_image = (uint16_t *)calloc(CAMERA_WIDTH * CAMERA_HEIGHT, sizeof(uint16_t));
}

void HikvisionIRCamera::release_memery(camera_handle_t* camera_handle, raw_data_handle_t* raw_data)
{
    // 停止线程（如果在运行）
    if (raw_data->running_) {
        stop_frame_processor_thread();
    }

    if (camera_handle->running_) {
        stop_check_login_status_thread();
    }

    // 释放 frame_buffer
    if (raw_data->frame_buffer) {
        free(raw_data->frame_buffer);
        raw_data->frame_buffer = nullptr;
    }

    // 释放 grey 8bit 图像
    if (raw_data->grey_8bit_image) {
        free(raw_data->grey_8bit_image);
        raw_data->grey_8bit_image = nullptr;
    }

    // 释放 grey 16bit 图像
    if (raw_data->grey_16bit_image) {
        free(raw_data->grey_16bit_image);
        raw_data->grey_16bit_image = nullptr;
    }

    // 清空帧队列
    {
        std::lock_guard<std::mutex> lock(raw_data->frame_queue_mutex);
        std::queue<std::vector<uint16_t>> empty;
        std::swap(raw_data->frame_queue, empty);
    }
}

void CALLBACK HikvisionIRCamera::g_RealDataCallBack_V30(LONG lRealHandle, DWORD dwDataType, BYTE *pBuffer, DWORD dwBufSize, void* pUser)
{
    HikvisionIRCamera* hikvision_ir_camera = static_cast<HikvisionIRCamera*>(pUser);
    
    if (dwDataType == NET_DVR_STREAMDATA)
    {
        {
            std::lock_guard<std::mutex> lock(hikvision_ir_camera->raw_data.frame_buffer_mutex);
            hikvision_ir_camera->write_to_buffer(&hikvision_ir_camera->raw_data, pBuffer, dwBufSize);
            // hikvision_ir_camera->find_magic_from_buffer(&hikvision_ir_camera->raw_data);
            // if (!hikvision_ir_camera->raw_data.magic_positions_queue.empty())
            //     hikvision_ir_camera->raw_data.frame_sem.release();
        }
    }
}

void HikvisionIRCamera::write_to_buffer(raw_data_handle_t* raw_data, BYTE *pBuffer, DWORD dwBufSize)
{
    size_t buffer_size = raw_data->buffer_size;
    uint8_t* frame_buffer = raw_data->frame_buffer;
    size_t& buffer_start = raw_data->frame_data_start_index;
    size_t& buffer_end = raw_data->frame_data_end_index;
    bool& is_buffer_empty = raw_data->is_buffer_empty;

    size_t free_space;
    if (is_buffer_empty) {
        free_space = buffer_size - 1;  // 保留一个位置不用，以区分满/空状态
    } else if ((buffer_end + 1) % buffer_size == buffer_start) {
        free_space = 0;  // 缓冲区已满
    } else {
        free_space = (buffer_start - buffer_end - 1 + buffer_size) % buffer_size;
    }

    if (dwBufSize > free_space)
    {
        std::cerr << "Warning: Buffer overrun! Overwriting old frames." << std::endl;
        buffer_start = (buffer_end + dwBufSize) % buffer_size;
    }

    size_t tail_space = buffer_size - buffer_end;
    if (dwBufSize <= tail_space)
    {
        memcpy(frame_buffer + buffer_end, pBuffer, dwBufSize);
    } 
    else
    {
        memcpy(frame_buffer + buffer_end, pBuffer, tail_space);
        memcpy(frame_buffer, pBuffer + tail_space, dwBufSize - tail_space);
    }

    is_buffer_empty = false;
    if (buffer_end == buffer_start)
    {
        buffer_start = (buffer_start + 1) % buffer_size;
    }
    buffer_end = (buffer_end + dwBufSize) % buffer_size;
}

void HikvisionIRCamera::find_magic_from_buffer(raw_data_handle_t* raw_data)
{
    const uint8_t* MAGIC = raw_data->MAGIC_BYTES;
    size_t buffer_size = raw_data->buffer_size;
    uint8_t* frame_buffer = raw_data->frame_buffer;
    size_t& buffer_start = raw_data->frame_data_start_index;
    size_t& buffer_end = raw_data->frame_data_end_index;
    bool& is_buffer_empty = raw_data->is_buffer_empty;

    int magic_found_count = 0;
    size_t free_space;

    if (is_buffer_empty) {
        free_space = buffer_size - 1;  // 保留一个位置不用，以区分满/空状态
    } else if ((buffer_end + 1) % buffer_size == buffer_start) {
        free_space = 0;  // 缓冲区已满
    } else {
        free_space = (buffer_start - buffer_end - 1 + buffer_size) % buffer_size;
    }
    size_t data_size = buffer_size - free_space;
    if (data_size < 222856)
    {
        return;
    }

    for (size_t i = 0; i <= data_size - 4; ++i)
    {
        bool match = true;
        for (size_t j = 0; j < 4; ++j)
        {
            size_t idx = (buffer_start + i + j) % buffer_size;
            if (frame_buffer[idx] != MAGIC[j])
            {
                match = false;
                break;
            }
        }
        if (match)
        {
            size_t found_index = (buffer_start + i) % buffer_size;
            if (magic_found_count == 0)
            {
                raw_data->pos.start = found_index;
                magic_found_count = 1;
            }
            else if (magic_found_count == 1)
            {
                raw_data->pos.end = found_index;
                buffer_start = found_index;
                magic_found_count = 2;
                // {
                    // std::lock_guard<std::mutex> queue_lock(raw_data->magic_queue_mutex);
                raw_data->magic_positions_queue.push(raw_data->pos);
                // }
                // std::cout << "找到Magic bytes found at: " << raw_data->pos.start << ", " << raw_data->pos.end << std::endl;
            }
        }
    }
}

void HikvisionIRCamera::start_get_raw_data(camera_handle_t* camera_handle, raw_data_handle_t* raw_data)
{
    //全屏测温参数配置
    //输入参数
    NET_DVR_XML_CONFIG_INPUT struInput = { 0 };
    struInput.dwSize = sizeof(struInput);

    //输出参数
    NET_DVR_XML_CONFIG_OUTPUT struOutputParam = { 0 };
    struOutputParam.dwSize = sizeof(struOutputParam);

    char szUrl[512]; //URL
    char szOutput[8 * 1024] = { 0 }; //输出结果
    char szStatusBuf[1024] = { 0 };  //状态信息
    char pBuf[2 * 1024] = { 0 };

    //获取参数
    memset(szUrl, 0, sizeof(szUrl));
    sprintf(szUrl, "%s", "GET /ISAPI/Thermal/channels/1/streamParam\r\n");

    //不同功能对应不同URL
    struInput.lpRequestUrl = szUrl;
    struInput.dwRequestUrlLen = strlen(szUrl);

    //获取时输入为空
    struInput.lpInBuffer = NULL;
    struInput.dwInBufferSize = 0;

    //分配输出内存
    memset(szOutput, 0, sizeof(szOutput));
    struOutputParam.lpOutBuffer = szOutput;
    struOutputParam.dwOutBufferSize = sizeof(szOutput);

    //输出状态
    memset(szStatusBuf, 0, sizeof(szStatusBuf));
    struOutputParam.lpStatusBuffer = szStatusBuf;
    struOutputParam.dwStatusSize = sizeof(szStatusBuf);
   
    if (!NET_DVR_STDXMLConfig(camera_handle->lUserID, &struInput, &struOutputParam))
    {
        printf("NET_DVR_STDXMLConfig failed, error code: %d\n", NET_DVR_GetLastError());
    }
    else
    {
        printf("NET_DVR_STDXMLConfig successfully!\n");
        printf("\n%s", szOutput);
    }
    
    //设置参数
    memset(szUrl, 0, sizeof(szUrl));
    sprintf(szUrl, "%s", "PUT /ISAPI/Thermal/channels/1/streamParam\r\n");

    //输入JSON数据
    memset(pBuf, 0, sizeof(pBuf));
    strcpy(pBuf, "<ThermalStreamParam xmlns=\"http://www.isapi.org/ver20/XMLSchema\" version=\"2.0\">"
        "<videoCodingType>real-time_raw_data</videoCodingType>"
        "</ThermalStreamParam>");

    //输入参数
    struInput.lpInBuffer = pBuf;
    struInput.dwInBufferSize = sizeof(pBuf);

    //输出结果
    memset(szOutput, 0, sizeof(szOutput));
    struOutputParam.lpOutBuffer = szOutput;
    struOutputParam.dwOutBufferSize = sizeof(szOutput);

    //输出状态
    memset(szStatusBuf, 0, sizeof(szStatusBuf));
    struOutputParam.lpStatusBuffer = szStatusBuf;
    struOutputParam.dwStatusSize = sizeof(szStatusBuf);

    if (!NET_DVR_STDXMLConfig(camera_handle->lUserID, &struInput, &struOutputParam))
    {
        printf("NET_DVR_STDXMLConfig failed, error code: %d\n", NET_DVR_GetLastError());
    }
    else
    {
        printf("NET_DVR_STDXMLConfig successfully!\n");
        printf("lpOutBuffer: \n%s", szOutput);
        printf("lpStatusBuffer: \n%s", szStatusBuf);
    }
    

    //获取参数
    memset(szUrl, 0, sizeof(szUrl));
    sprintf(szUrl, "%s", "GET /ISAPI/Thermal/channels/1/thermometry/pixelToPixelParam\r\n");

    //不同功能对应不同URL
    struInput.lpRequestUrl = szUrl;
    struInput.dwRequestUrlLen = strlen(szUrl);

    //获取时输入为空
    struInput.lpInBuffer = NULL;
    struInput.dwInBufferSize = 0;

    //分配输出内存
    memset(szOutput, 0, sizeof(szOutput));
    struOutputParam.lpOutBuffer = szOutput;
    struOutputParam.dwOutBufferSize = sizeof(szOutput);

    //输出状态
    memset(szStatusBuf, 0, sizeof(szStatusBuf));
    struOutputParam.lpStatusBuffer = szStatusBuf;
    struOutputParam.dwStatusSize = sizeof(szStatusBuf);
    
    if (!NET_DVR_STDXMLConfig(camera_handle->lUserID, &struInput, &struOutputParam))
    {
        printf("NET_DVR_STDXMLConfig failed, error code: %d\n", NET_DVR_GetLastError());
    }
    else
    {
        printf("NET_DVR_STDXMLConfig successfully!\n");
        printf("\n%s", szOutput);
    }
    //设置参数
    memset(szUrl, 0, sizeof(szUrl));
    sprintf(szUrl, "%s", "PUT /ISAPI/Thermal/channels/1/thermometry/pixelToPixelParam\r\n");
    struInput.lpRequestUrl = szUrl;
    struInput.dwRequestUrlLen = strlen(szUrl);

    //输入JSON数据
    memset(pBuf, 0, sizeof(pBuf));
    //帧数*100即为maxFrameRate字段的值
    strcpy(pBuf, "<?xml version=\"1.0\" encoding=\"UTF - 8\"?>"
        "<PixelToPixelParam version = \"2.0\" xmlns = \"http://www.hikvision.com/ver20/XMLSchema\">"
        "<id>1</id>"
        "<maxFrameRate>400</maxFrameRate>"
        "<reflectiveEnable>false</reflectiveEnable>"
        "<reflectiveTemperature>20.00</reflectiveTemperature>"
        "<emissivity>0.98</emissivity>"
        "<distance>0.5</distance>"
        "<refreshInterval>50</refreshInterval>"
        "<distanceUnit>centimeter</distanceUnit>"
        "<temperatureDataLength>4</temperatureDataLength>"
        "<JpegPictureWithAppendData>"
        "<jpegPicEnabled>true</jpegPicEnabled>"
        "<visiblePicEnabled>true</visiblePicEnabled>"
        "</JpegPictureWithAppendData>"
        "</PixelToPixelParam>");
    struInput.lpInBuffer = pBuf;
    struInput.dwInBufferSize = sizeof(pBuf);

    //输出结果
    memset(szOutput, 0, sizeof(szOutput));
    struOutputParam.lpOutBuffer = szOutput;
    struOutputParam.dwOutBufferSize = sizeof(szOutput);

    //输出状态
    memset(szStatusBuf, 0, sizeof(szStatusBuf));
    struOutputParam.lpStatusBuffer = szStatusBuf;
    struOutputParam.dwStatusSize = sizeof(szStatusBuf);
    
    if (!NET_DVR_STDXMLConfig(camera_handle->lUserID, &struInput, &struOutputParam))
    {
        printf("NET_DVR_STDXMLConfig failed, error code: %d\n", NET_DVR_GetLastError());
    }
    else
    {
        printf("NET_DVR_STDXMLConfig successfully!\n");
        printf("lpOutBuffer: \n%s", szOutput);
        printf("lpStatusBuffer: \n%s", szStatusBuf);
    }

    NET_DVR_PREVIEWINFO struPlayInfo = { 0 };
    struPlayInfo.hPlayWnd = NULL;         //需要SDK解码时句柄设为有效值，仅取流不解码时可设为空
    struPlayInfo.lChannel = 1;       //预览通道号
    struPlayInfo.dwStreamType = 0;       //0-主码流，1-子码流，2-码流3，3-码流4，以此类推
    struPlayInfo.dwLinkMode = 0;       //0- TCP方式，1- UDP方式，2- 多播方式，3- RTP方式，4-RTP/RTSP，5-RSTP/HTTP
    struPlayInfo.byVideoCodingType = 1;  //码流数据编解码类型：0- 通用编码数据，1- 热成像数据
    struPlayInfo.bBlocked = 1;       //0- 非阻塞取流，1- 阻塞取流
    
    camera_handle->lRealPlayHandle = NET_DVR_RealPlay_V40(camera_handle->lUserID, &struPlayInfo, g_RealDataCallBack_V30, this);
    if (camera_handle->lRealPlayHandle < 0)
    {
        printf("NET_DVR_RealPlay_V40 error, %d\n", NET_DVR_GetLastError());
        std::cerr << "启动播放失败, 重试中..." << std::endl;
    }
}

void HikvisionIRCamera::stop_get_raw_data(camera_handle_t* camera_handle)
{
    std::cout << "stop get raw data." << std::endl;
    NET_DVR_StopRealPlay(camera_handle->lRealPlayHandle);
    camera_handle->lRealPlayHandle = -1;      
}

bool HikvisionIRCamera::is_connect(const std::string& ipAddress, int port, int timeoutSec = 5)
{
    int sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd < 0) {
        std::cerr << "Error creating socket" << std::endl;
        return false;
    }

    struct sockaddr_in serv_addr;
    memset(&serv_addr, 0, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(port);
    if (inet_pton(AF_INET, ipAddress.c_str(), &serv_addr.sin_addr) <= 0) {
        std::cerr << "Invalid IP address" << std::endl;
        close(sockfd);
        return false;
    }

    // 设置超时时间
    struct timeval timeout;
    timeout.tv_sec = timeoutSec;
    timeout.tv_usec = 0;
    setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout, sizeof(timeout));
    setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (const char*)&timeout, sizeof(timeout));

    int result = connect(sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr));
    close(sockfd);

    return result == 0;
}

void HikvisionIRCamera::check_login_status_thread_function(camera_handle_t* camera_handle, raw_data_handle_t* raw_data)
{
    bool previous_connect_status = false;
    bool current_connect_status = false;
    while (camera_handle->running_) 
    {
        current_connect_status = is_connect(camera_handle->loginInfo.sDeviceAddress, camera_handle->loginInfo.wPort);
        if (current_connect_status == true && previous_connect_status == false)
        {
            NET_DVR_Init();
            std::cout << "摄像头已连接，登录中..." << std::endl;
            camera_handle->lUserID = NET_DVR_Login_V40(&camera_handle->loginInfo, &camera_handle->deviceInfo);
            if (camera_handle->lUserID < 0)
            {
                std::cout << "登录失败，重复尝试..." << std::endl;
                std::cerr << "NET_DVR_Login_V40, error code:" << NET_DVR_GetLastError() << std::endl;
                NET_DVR_Cleanup();
                previous_connect_status = false;
                continue;
            }
            start_get_raw_data(camera_handle, raw_data);
            if (0 > camera_handle->lRealPlayHandle) {
                // 退出登录、释放资源
                NET_DVR_Logout(camera_handle->lUserID);
                NET_DVR_Cleanup();
                previous_connect_status = false;
                continue;
            }
            std::cout << "登录成功！" << std::endl;
        }
        else if (current_connect_status == false && previous_connect_status == false)
        {
            std::cout << "摄像头断开网络连接..." << std::endl;
            NET_DVR_Cleanup();
            camera_handle->lUserID = -1;
            stop_get_raw_data(camera_handle);
        }
        else
        {
            std::cout << "摄像头连接正常" << std::endl;
        }
        previous_connect_status = current_connect_status;
        // 3秒检测一次
        std::this_thread::sleep_for(std::chrono::seconds(60));
    }
    stop_get_raw_data(camera_handle);

    // 退出登录、释放资源
    NET_DVR_Logout(camera_handle->lUserID);
    NET_DVR_Cleanup();
    std::cout << "check_login_status_thread is stopping." << std::endl;
}

void HikvisionIRCamera::start_check_login_status_thread()
{
    camera_handle.running_ = true;
    camera_handle.connect_status_check_thread_ = std::thread(&HikvisionIRCamera::check_login_status_thread_function, this, &camera_handle, &raw_data);
}

void HikvisionIRCamera::stop_check_login_status_thread()
{
    camera_handle.running_ = false;
    camera_handle.connect_status_check_thread_.join();
}

int HikvisionIRCamera::raw_data_to_grey_16bit(raw_data_handle_t* raw_data, size_t start_pos, size_t end_pos)
{
    size_t dataSize;
    if (end_pos > start_pos)
        dataSize = end_pos - start_pos - (384 * 2 * 2);
    else{
        dataSize = (raw_data->buffer_size - start_pos) + end_pos - (384 * 2 * 2);
    }
        
    // dataSize = end_pos - start_pos - (384 * 2 * 2);
    if (dataSize != 221184) 
    {
        std::cerr << "Error: Data size mismatch. Expected 221184 bytes, got " << dataSize << " bytes." << std::endl;
        return -1;
    }

    if  (end_pos > start_pos)
    {
        for (size_t i = 0; i < (CAMERA_WIDTH * CAMERA_HEIGHT); ++i) 
        {
            uint8_t lowByte = raw_data->frame_buffer[start_pos + 2 * i];
            uint8_t highByte = raw_data->frame_buffer[start_pos + 2 * i + 1];
            raw_data->grey_16bit_image[i] = static_cast<uint16_t>(highByte) << 8 | lowByte;
        }
    }
    else
    {
        // 计算从start_pos到buffer末尾有多少完整的16位像素（每个像素2字节）
        size_t complete_pixels = (raw_data->buffer_size - start_pos) / 2;
        
        // 处理第一部分（从start_pos到buffer末尾）
        for (size_t i = 0; i < complete_pixels; ++i) 
        {
            uint8_t lowByte = raw_data->frame_buffer[start_pos + 2 * i];
            uint8_t highByte = raw_data->frame_buffer[start_pos + 2 * i + 1];
            raw_data->grey_16bit_image[i] = static_cast<uint16_t>(highByte) << 8 | lowByte;
        }
        
        // 检查是否有单个字节剩余（奇数情况）
        bool has_remainder = ((raw_data->buffer_size - start_pos) % 2 == 1);
        
        // 处理跨越边界的单个像素（如果有）
        if (has_remainder) {
            uint8_t lowByte = raw_data->frame_buffer[raw_data->buffer_size - 1];
            uint8_t highByte = raw_data->frame_buffer[0];
            raw_data->grey_16bit_image[complete_pixels] = static_cast<uint16_t>(highByte) << 8 | lowByte;
            
            // 第二部分从索引1开始（因为索引0已用于跨边界像素）
            for (size_t i = 0; i < (end_pos - 1) / 2; ++i) 
            {
                uint8_t lowByte = raw_data->frame_buffer[1 + 2 * i];
                uint8_t highByte = raw_data->frame_buffer[1 + 2 * i + 1];
                raw_data->grey_16bit_image[complete_pixels + 1 + i] = static_cast<uint16_t>(highByte) << 8 | lowByte;
            }
        } 
        else {
            // 正常处理第二部分（从buffer开头到end_pos）
            for (size_t i = 0; i < end_pos / 2; ++i) 
            {
                uint8_t lowByte = raw_data->frame_buffer[2 * i];
                uint8_t highByte = raw_data->frame_buffer[2 * i + 1];
                raw_data->grey_16bit_image[complete_pixels + i] = static_cast<uint16_t>(highByte) << 8 | lowByte;
            }
        }
    }
    
    return 0;
}

void HikvisionIRCamera::frame_processor_thread_function(raw_data_handle_t* raw_data)
{
    MagicPosition pos;
    while (raw_data->running_)
    {
        // raw_data->frame_sem.acquire();
        if (!raw_data->running_) {break;}
        {
            std::lock_guard<std::mutex> lock(raw_data->frame_buffer_mutex);
            find_magic_from_buffer(raw_data);
            if (!raw_data->magic_positions_queue.empty())
            {
                // std::cout << "队列长度为: " << raw_data->magic_positions_queue.size() << std::endl;
                pos = raw_data->magic_positions_queue.front();
                raw_data->magic_positions_queue.pop();
                size_t start_pos = pos.start + 132;
                size_t end_pos = pos.end;
                // std::cout << "队列长度为: " << raw_data->magic_positions_queue.size() << std::endl;
                // std::cout << "frame_processor_thread_function: " << start_pos << ", end_pos: " << end_pos << std::endl;
            
                // std::cout << start_pos << ", " << end_pos << ", " << raw_data->buffer_size << std::endl;
                if (start_pos <= raw_data->buffer_size && end_pos <= raw_data->buffer_size)
                {
                    // std::cout << "enter raw_data_to_grey_16bit!" << std::endl;
                    if (raw_data_to_grey_16bit(raw_data, start_pos, end_pos) < 0) {continue;}

                    #ifdef USE_FRAME_QUEUE
                    std::lock_guard<std::mutex> queueLock(raw_data->frame_queue_mutex);
                    if (raw_data->frame_queue.size() >= raw_data->max_frame_queue_size)
                    {
                        raw_data->frame_queue.pop(); // 队列满时，移除最旧的帧
                    }
                    std::vector<uint16_t> frame_copy(raw_data->grey_16bit_image, raw_data->grey_16bit_image + CAMERA_WIDTH * CAMERA_HEIGHT);
                    raw_data->frame_queue.push(std::move(frame_copy));
                    #endif
                }
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(6));
    }
}

void HikvisionIRCamera::start_frame_processor_thread()
{
    raw_data.running_ = true;
    raw_data.frame_processor_thread_= std::thread(&HikvisionIRCamera::frame_processor_thread_function, this, &raw_data);
}

void HikvisionIRCamera::stop_frame_processor_thread()
{
    raw_data.running_ = false;
    raw_data.frame_processor_thread_.join();
}

int HikvisionIRCamera::get_grey_16bit_from_queue(std::vector<uint16_t>& frame)
{
    if (frame.size() != CAMERA_WIDTH * CAMERA_HEIGHT) {
        std::cerr << "Error: Frame size mismatch. Expected " << CAMERA_WIDTH * CAMERA_HEIGHT << " bytes." << std::endl;
        return -1;
    }

    std::lock_guard<std::mutex> lock(raw_data.frame_queue_mutex);

    if (raw_data.frame_queue.empty())
    {
        return -1;
    }

    frame = std::move(raw_data.frame_queue.front());
    raw_data.frame_queue.pop();

    return 0;
}

int HikvisionIRCamera::get_grey_16bit_image(std::vector<uint16_t>& frame)
{
    if (frame.size() != CAMERA_WIDTH * CAMERA_HEIGHT) {
        std::cerr << "Error: Frame size mismatch. Expected " << CAMERA_WIDTH * CAMERA_HEIGHT << " bytes." << std::endl;
        return -1;
    }

    std::lock_guard<std::mutex> lock(raw_data.grey_image_mutex);

    if (!raw_data.grey_16bit_image)
        return -1;

    std::copy(raw_data.grey_16bit_image, raw_data.grey_16bit_image + CAMERA_WIDTH * CAMERA_HEIGHT, frame.begin());
    return 0;
}

int HikvisionIRCamera::get_grey_8bit_image(std::vector<uint8_t>& frame)
{
    if (frame.size() != CAMERA_WIDTH * CAMERA_HEIGHT) {
        std::cerr << "Error: Frame size mismatch. Expected " << CAMERA_WIDTH * CAMERA_HEIGHT << " bytes." << std::endl;
        return -1;
    }

    std::lock_guard<std::mutex> lock(raw_data.grey_image_mutex);
    uint16_t max_val = 0;
    uint16_t min_val = UINT16_MAX;

    for (size_t i = 0; i < CAMERA_WIDTH * CAMERA_HEIGHT; ++i) {
        uint16_t pixel_value = raw_data.grey_16bit_image[i];
        if (pixel_value > max_val) {
            max_val = pixel_value;
        }
        if (pixel_value < min_val) {
            min_val = pixel_value;
        }
    }

    if (max_val == min_val) {
        return -1;
    }
    else {
        for (size_t i = 0; i < CAMERA_WIDTH * CAMERA_HEIGHT; ++i)
        {
            frame[i] = static_cast<uint8_t>((raw_data.grey_16bit_image[i] - min_val) * 255 / (max_val - min_val));
        }
    }
    
    return 0;
}